% INS/GNSS松组合卡尔曼滤波 - 匀速圆周运动（改进初始状态）
clear all; close all; clc;
% 参数设置
dt = 0.1;           % 时间步长 (s)
total_time = 30;    % 总时长 (s)
steps = total_time / dt; % 总步数
r = 50;             % 圆周半径 (m)
omega = 0.3;        % 角速度 (rad/s)

% 过程噪声和观测噪声参数
accel_noise = 0.1;  % 加速度计噪声 (m/s^2)
gyro_noise = 0.01;  % 陀螺仪噪声 (rad/s)
gnss_noise = 1.0;   % GNSS位置噪声 (m)

% 生成真实圆周运动轨迹
t = 0:dt:total_time-dt;
x = r * cos(omega * t);     % X位置
y = r * sin(omega * t);     % Y位置
vx = -r * omega * sin(omega * t); % X方向速度
vy = r * omega * cos(omega * t);  % Y方向速度
true_states = [x' y' vx' vy']; % 真实状态矩阵

% 初始状态（用于卡尔曼滤波器初始化）
initial_state = [r; 0; 0; r*omega]; % 使用真实初始状态

% 传感器测量模拟
accel_measurements = zeros(steps, 2);
gyro_measurements = zeros(steps, 1);
gnss_measurements = zeros(steps, 2);

for i = 1:steps
    % 向心加速度计算 (a = omega^2 * r)
    centripetal_accel = omega^2 * r;
    
    % 加速度矢量 (指向圆心)
    direction = true_states(i, 1:2) / norm(true_states(i, 1:2));
    true_accel = -centripetal_accel * direction;
    
    % 添加噪声
    accel_measurements(i, :) = true_accel + randn(1,2) * accel_noise;
    gyro_measurements(i) = omega + randn * gyro_noise;
    
    % GNSS位置测量
    gnss_measurements(i, :) = true_states(i, 1:2) + randn(1,2) * gnss_noise;
end





% 运行改进的卡尔曼滤波器

kf = ImprovedKalmanFilter(dt, initial_state);
estimated_states = zeros(steps, 4);

% 记录协方差变化用于分析
position_covariance = zeros(steps, 1);
velocity_covariance = zeros(steps, 1);

for i = 1:steps
   
    kf = kf.predict(accel_measurements(i, :));
    
    % 使用GNSS数据进行更新 (每5步更新一次)
    if mod(i, 5) == 0
        kf = kf.update(gnss_measurements(i, :));
    end
    
    % 存储估计状态
    estimated_states(i, :) = kf.state';
    
    % 记录协方差信息
    position_covariance(i) = sqrt(kf.P(1,1) + kf.P(2,2)); % 位置标准差
    velocity_covariance(i) = sqrt(kf.P(3,3) + kf.P(4,4)); % 速度标准差
end

% 结果可视化
figure('Position', [100, 100, 1400, 1000]);

% 1. 轨迹对比
subplot(3, 2, 1);
plot(true_states(:,1), true_states(:,2), 'b-', 'LineWidth', 1.5);
hold on;
plot(estimated_states(:,1), estimated_states(:,2), 'r--', 'LineWidth', 1.5);
scatter(gnss_measurements(:,1), gnss_measurements(:,2), 20, 'g', 'filled', 'MarkerFaceAlpha', 0.3);
axis equal;
xlabel('X位置 (m)');
ylabel('Y位置 (m)');
title('轨迹对比 (改进初始状态)');
legend('真实轨迹', '估计轨迹', 'GNSS测量', 'Location', 'best');
grid on;
hold off;

subplot(3, 2, 2);
plot(true_states(1:50,1), true_states(1:50,2), 'b-', 'LineWidth', 2);
hold on;
plot(estimated_states(1:50,1), estimated_states(1:50,2), 'r--', 'LineWidth', 2);
scatter(gnss_measurements(1:50,1), gnss_measurements(1:50,2), 30, 'g', 'filled', 'MarkerFaceAlpha', 0.5);
axis equal;
xlabel('X位置 (m)');
ylabel('Y位置 (m)');
title('初始阶段放大 (前5秒)');
legend('真实轨迹', '估计轨迹', 'GNSS测量', 'Location', 'best');
grid on;
hold off;

% 3. 位置误差分析
position_error = vecnorm(true_states(:, 1:2) - estimated_states(:, 1:2), 2, 2);
subplot(3, 2, 3);
plot(position_error, 'LineWidth', 1.5);
hold on;
plot(3 * position_covariance, 'r--', 'LineWidth', 1.0); % 3σ边界
plot(-3 * position_covariance, 'r--', 'LineWidth', 1.0);
xlabel('时间步');
ylabel('误差 (m)');
title('位置误差及3σ边界');
legend('位置误差', '3σ边界');
grid on;
ylim([-5, 5]);

% 4. 速度对比
time_axis = 0:dt:total_time-dt;
subplot(3, 2, 4);
plot(time_axis, true_states(:,3), 'b-', 'LineWidth', 1.5);
hold on;
plot(time_axis, estimated_states(:,3), 'r--', 'LineWidth', 1.5);
plot(time_axis, true_states(:,4), 'g-', 'LineWidth', 1.5);
plot(time_axis, estimated_states(:,4), 'm--', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('速度 (m/s)');
title('速度分量对比');
legend('真实Vx', '估计Vx', '真实Vy', '估计Vy', 'Location', 'best');
grid on;
hold off;

% 5. 速度误差
velocity_error = vecnorm(true_states(:, 3:4) - estimated_states(:, 3:4), 2, 2);
subplot(3, 2, 5);
plot(time_axis, velocity_error, 'LineWidth', 1.5);
hold on;
plot(time_axis, 3 * velocity_covariance, 'r--', 'LineWidth', 1.0); % 3σ边界
xlabel('时间 (s)');
ylabel('误差 (m/s)');
title('速度误差及3σ边界');
legend('速度误差', '3σ边界');
grid on;

% 6. 协方差收敛分析
subplot(3, 2, 6);
semilogy(time_axis, position_covariance, 'b-', 'LineWidth', 1.5);
hold on;
semilogy(time_axis, velocity_covariance, 'r-', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('标准差');
title('协方差收敛分析');
legend('位置标准差', '速度标准差');
grid on;

% 性能指标计算

fprintf('改进滤波器性能指标:\n');
fprintf('平均位置误差: %.4f m\n', mean(position_error));
fprintf('最大位置误差: %.4f m\n', max(position_error));
fprintf('平均速度误差: %.4f m/s\n', mean(velocity_error));
fprintf('初始位置误差: %.4f m\n', position_error(1));
fprintf('初始速度误差: %.4f m/s\n', velocity_error(1));

% 计算收敛时间（误差首次降到1m以下）
convergence_index = find(position_error < 1.0, 1);
if ~isempty(convergence_index)
    convergence_time = convergence_index * dt;
    fprintf('收敛时间 (位置误差 < 1m): %.2f 秒\n', convergence_time);
else
    fprintf('系统未在仿真时间内收敛到1m误差以内\n');
end